/*******************************************************************************
 * File Name: GPS_UBLOX.cpp
 *
 * Version: 1.0
 *
 * Description:
 * 	
 * Author: ppks
 *
 * Notes:
 * 	
 *
 ********************************************************************************
 * Copyright (<year>)
 ********************************************************************************
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 ********************************************************************************/

#include "common.h"
#include "GPS_UBLOX.h"
#include "GPS.h"
#include "Serial.h"
#include "FlightController.h"

/********************************************************************************/
/* Constants Definition                                                         */
/********************************************************************************/
/* Protocol Message Structure :
 * HEAD1 | HEAD2 | CLASS | ID | LENGTH | PAYLOAD | CRC_A | CRC_B
 *  1B      1B      1B    1B     2B     LENGTH B    1B      1B   */
prog_char UBLOX_INIT[] PROGMEM = {                          /* PROGMEM array must be outside any function !!! */
     0xB5,0x62,0x06,0x01,0x03,0x00,0xF0,0x05,0x00,0xFF,0x19,                            /* disable all default NMEA messages */
     0xB5,0x62,0x06,0x01,0x03,0x00,0xF0,0x03,0x00,0xFD,0x15,
     0xB5,0x62,0x06,0x01,0x03,0x00,0xF0,0x01,0x00,0xFB,0x11,
     0xB5,0x62,0x06,0x01,0x03,0x00,0xF0,0x00,0x00,0xFA,0x0F,
     0xB5,0x62,0x06,0x01,0x03,0x00,0xF0,0x02,0x00,0xFC,0x13,
     0xB5,0x62,0x06,0x01,0x03,0x00,0xF0,0x04,0x00,0xFE,0x17,
     0xB5,0x62,0x06,0x01,0x03,0x00,0x01,0x02,0x01,0x0E,0x47,                            /* set POSLLH MSG rate */
     0xB5,0x62,0x06,0x01,0x03,0x00,0x01,0x03,0x01,0x0F,0x49,                            /* set STATUS MSG rate */
     0xB5,0x62,0x06,0x01,0x03,0x00,0x01,0x06,0x01,0x12,0x4F,                            /* set SOL MSG rate */
     0xB5,0x62,0x06,0x01,0x03,0x00,0x01,0x12,0x01,0x1E,0x67,                            /* set VELNED MSG rate */
     0xB5,0x62,0x06,0x16,0x08,0x00,0x03,0x07,0x03,0x00,0x51,0x08,0x00,0x00,0x8A,0x41,   /* set WAAS to EGNOS */
     0xB5,0x62,0x06,0x08,0x06,0x00,0xC8,0x00,0x01,0x00,0x01,0x00,0xDE,0x6A 				/* set rate to 5Hz */
};

/********************************************************************************/
/* Local Structures Definition                                                  */
/********************************************************************************/
struct ubx_header {
    uint8_t preamble1;
    uint8_t preamble2;
    uint8_t msg_class;
    uint8_t msg_id;
    uint16_t length;
};

struct ubx_nav_posllh {
    uint32_t time;  /* GPS msToW */
    int32_t longitude;
    int32_t latitude;
    int32_t altitude_ellipsoid;
    int32_t altitude_msl;
    uint32_t horizontal_accuracy;
    uint32_t vertical_accuracy;
};

struct ubx_nav_solution {
    uint32_t time;
    int32_t time_nsec;
    int16_t week;
    uint8_t fix_type;
    uint8_t fix_status;
    int32_t ecef_x;
    int32_t ecef_y;
    int32_t ecef_z;
    uint32_t position_accuracy_3d;
    int32_t ecef_x_velocity;
    int32_t ecef_y_velocity;
    int32_t ecef_z_velocity;
    uint32_t speed_accuracy;
    uint16_t position_DOP;
    uint8_t res;
    uint8_t satellites;
    uint32_t res2;
};

struct ubx_nav_velned {
    uint32_t time;  /* GPS msToW */
    int32_t ned_north;
    int32_t ned_east;
    int32_t ned_down;
    uint32_t speed_3d;
    uint32_t speed_2d;
    int32_t heading_2d;
    uint32_t speed_accuracy;
    uint32_t heading_accuracy;
};


/********************************************************************************/
/* Global Variables Declaration                                                 */
/********************************************************************************/

/********************************************************************************/
/* Local Variables Declaration                                                  */
/********************************************************************************/
/* Receive buffer */
static union {
    ubx_nav_posllh posllh;
    ubx_nav_solution solution;
    ubx_nav_velned velned;
    uint8_t bytes[];
} _buffer;

/* Packet checksum accumulators */
static uint8_t UBX_checksum_a;
static uint8_t UBX_checksum_b;

/* State machine state */
static uint8_t  UBX_Msg_Id;
static uint16_t UBX_PayloadLength;
static uint16_t UBX_PayloadCounter;

/* TODO: rename */
static uint8_t UBX_Class_Id;
static uint8_t UBX_Fix_OK;

/********************************************************************************/
/* Local Functions Declaration                                                  */
/********************************************************************************/
static void UBX_Configure(void);
static void UBX_SerialInit(void);
static void GPS_SerialPrint(const char PROGMEM * str);
static bool UBX_ParseData(void);

/********************************************************************************/
/* Function Code                                                                */
/********************************************************************************/
/********************************************************************************
* Function Name: GPS_UBX_Init
*********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*********************************************************************************/
void GPS_UBX_Init(void)
{
    /* Configure GPS */
    UBX_Configure();
}

/********************************************************************************
* Function Name: UBX_Configure
*********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*********************************************************************************/
static void UBX_Configure(void)
{
    /* Init Serial Communication Protocol */
    UBX_SerialInit();

    /* Configure the message rates */
    for (uint8_t i = 0; i < sizeof(UBLOX_INIT); i++)
    {
        /* send configuration data in UBX protocol */
        CommSerial_SendUint8(GPS_SERIAL_PORT, pgm_read_byte(UBLOX_INIT + i));

        /* Ad extra delay: simulating a 38400baud pace (or less), otherwise commands are not accepted by the device. */
        delay(5);
    }
}

/********************************************************************************
* Function Name: UBX_SerialInit
*********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*********************************************************************************/
static void UBX_SerialInit(void)
{
    /* List of available baudrates */
    uint32_t gps_baudrates[5] = {9600, 19200, 38400, 57600, 115200};

    CommSerial_Open(GPS_SERIAL_PORT, GPS_SERIAL_BAUDRATE);

    /* Wait 1 sec */
    delay(1000);

    /* Try to send message to the GPS on all supported baudrates */
    for (uint8_t i = 0; i < 5; i++)
    {
        CommSerial_Open(GPS_SERIAL_PORT, gps_baudrates[i]);          /* switch UART speed for sending SET BAUDRATE command (NMEA mode) */
    #if (GPS_SERIAL_BAUDRATE == 19200)
        GPS_SerialPrint(PSTR("$PUBX,41,1,0003,0001,19200,0*23\r\n"));      /* 19200 baud - minimal speed for 5Hz update rate */
    #elif (GPS_SERIAL_BAUDRATE == 38400)
        GPS_SerialPrint(PSTR("$PUBX,41,1,0003,0001,38400,0*26\r\n"));      /* 38400 baud */
    #elif (GPS_SERIAL_BAUDRATE == 57600)
        GPS_SerialPrint(PSTR("$PUBX,41,1,0003,0001,57600,0*2D\r\n"));      /* 57600 baud */
    #elif (GPS_SERIAL_BAUDRATE == 115200)
        GPS_SerialPrint(PSTR("$PUBX,41,1,0003,0001,115200,0*1E\r\n"));     /* 115200 baud */
    #endif

        /* Wait until the packet is completely transmitted + 10ms just in case to let the device react */
        while (!CommSerial_IsTXCompleted(GPS_SERIAL_PORT))
        {
            delay(10);
        }
    }

    delay(200);

    /* Open COM Port on the desired baudrate */
    CommSerial_Open(GPS_SERIAL_PORT, GPS_SERIAL_BAUDRATE);
}

/*******************************************************************************
* Function Name: GPS_SerialPrint
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static void GPS_SerialPrint(const char PROGMEM * str)
{
    char b;

    while (str && (b = pgm_read_byte(str++)))
    {
        CommSerial_SendUint8(GPS_SERIAL_PORT, b);
        delay(5); /* Ad extra delay: simulating a 38400baud pace (or less), otherwise commands are not accepted by the device. */
    }
}

/********************************************************************************
* Function Name: GPS_UBX_GetNewFrame
*********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*********************************************************************************/
bool GPS_UBX_GetNewFrame(void)
{
    /* Define state variable */
    static uint8_t state = 0;
    uint8_t data;

    /* Check if new data is available in the buffer and fet number of bytes available */
    uint8_t cc = CommSerial_IsNewDataAvailable(GPS_SERIAL_PORT);

    /* loop through all the bytes */
    while (cc--)
    {
        /* Read Byte from serial interface */
        data = CommSerial_ReadByteFromBuffer(GPS_SERIAL_PORT);

        switch(state)
        {
            case 1:
                if (PREAMBLE2 == data)
                {
                    state++;
                    break;
                }
                /* first byte was read by mistake - check the first byte again right away */
                state = 0;

            case 0:
                if (PREAMBLE1 == data)
                {
                    state++;
                }
                break;

            case 2:
                state++;
                UBX_Class_Id = data;
                UBX_checksum_b = UBX_checksum_a = data;  /* reset the checksum accumulators */
                break;

            case 3:
                state++;
                UBX_checksum_b += (UBX_checksum_a += data);  /* checksum byte */
                UBX_Msg_Id = data;
                break;

            case 4:
                state++;
                UBX_checksum_b += (UBX_checksum_a += data);  /* checksum byte */
                UBX_PayloadLength = data;  /* payload length low byte */
                break;
            case 5:
                state++;
                UBX_checksum_b += (UBX_checksum_a += data);  /* checksum byte */
                UBX_PayloadLength += (uint16_t)(data << 8);

                if (UBX_PayloadLength > 512)
                {
                    UBX_PayloadLength = 0;
                    state = 0;
                }
                UBX_PayloadCounter = 0;  /* prepare to receive payload */
                break;

            case 6:
                UBX_checksum_b += (UBX_checksum_a += data);  /* checksum byte */

                if (UBX_PayloadCounter < sizeof(_buffer))
                {
                    _buffer.bytes[UBX_PayloadCounter] = data;
                }

                if (++UBX_PayloadCounter == UBX_PayloadLength)
                {
                    state++;
                }
                break;

            case 7:
                state++;
                if (UBX_checksum_a != data)
                {
                    state = 0;  /* bad checksum */
                }
                break;

            case 8:
                state = 0;
                if (UBX_checksum_b != data)
                {
                    break;  /* bad checksum */
                }

                /* We received a complete packet - indicate that GPS is present */
                GPS_Present = 1;

                if (UBX_ParseData())
                {
                    return true;
                }
                break;
        } /* end switch */
    } /* End of while cc-- loop */

    return false;
}

/********************************************************************************
* Function Name: UBX_ParseData
*********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*********************************************************************************/
static bool UBX_ParseData(void)
{
    switch (UBX_Msg_Id)
    {
        case MSG_POSLLH:
            if (UBX_Fix_OK)
            {
                GPS_CurrentCoord[LON] = _buffer.posllh.longitude;
                GPS_CurrentCoord[LAT] = _buffer.posllh.latitude;
                GPS_Altitude   = _buffer.posllh.altitude_msl / 1000;      /* altitude in meteres */
            }
            globalFlags.GPS_FIX = UBX_Fix_OK;
            return true;        /* POSLLH message received, allow blink GUI icon and LED */
            break;

        case MSG_SOL:
            UBX_Fix_OK = 0;

            if((_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D || _buffer.solution.fix_type == FIX_2D))
            {
                UBX_Fix_OK = 1;
            }

            GPS_NumSat = _buffer.solution.satellites;
            break;

        case MSG_VELNED:
            GPS_Speed         = _buffer.velned.speed_2d;                        /* Speed in cm/s */
            GPS_GroundCourse = (uint16_t)(_buffer.velned.heading_2d / 10000);   /* Heading 2D deg * 100000 rescaled to deg * 10 */
            break;

        default:
            break;
    }
    return false;
}
